#include "tos_secure_config.h"

    EXPORT  port_int_disable
    EXPORT  port_int_enable

    EXPORT  port_cpsr_save
    EXPORT  port_cpsr_restore

    EXPORT  port_sched_start
    EXPORT  port_context_switch
    EXPORT  port_irq_context_switch

    EXPORT  port_clz

    EXPORT  PendSV_Handler

    IMPORT  k_curr_task
    IMPORT  k_next_task

    
#if TOS_CFG_TRUSTZONE_EN > 0u
	EXPORT port_secure_ctx_alloc
	EXPORT port_secure_ctx_free
	EXPORT port_secure_ctx_free_curr
	EXPORT port_secure_ctx_init

    EXTERN tos_secure_ctx_load
    EXTERN tos_secure_ctx_save

    IMPORT  k_curr_secure_ctx
#endif

SCB_VTOR        EQU     0xE000ED08
NVIC_INT_CTRL   EQU     0xE000ED04
NVIC_SYSPRI14   EQU     0xE000ED22
NVIC_PENDSV_PRI EQU     0xFF
NVIC_PENDSVSET  EQU     0x10000000

#if TOS_CFG_TRUSTZONE_EN > 0u
PORT_SVC_SECURE_CTX_ALLOC   EQU     0
PORT_SVC_SECURE_CTX_FREE    EQU     1
PORT_SVC_SECURE_CTX_INIT	EQU     2
K_SECURE_CTX_INVALID_ID     EQU     0
#endif



    AREA |.text|, CODE, READONLY, ALIGN=2
    THUMB
    REQUIRE8
    PRESERVE8


    GLOBAL port_int_disable
port_int_disable
    CPSID   I
    BX      LR


    GLOBAL port_int_enable
port_int_enable
    CPSIE   I
    BX      LR


    GLOBAL port_cpsr_save
port_cpsr_save
    MRS     R0, PRIMASK
    CPSID   I
    BX      LR


    GLOBAL port_cpsr_restore
port_cpsr_restore
    MSR     PRIMASK, R0
    BX      LR


    GLOBAL port_clz
port_clz
    CLZ     R0, R0
    BX      LR


    GLOBAL port_sched_start
port_sched_start
    CPSID   I

    ; set pendsv priority lowest
    ; otherwise trigger pendsv in port_irq_context_switch will cause a context switch in irq
    ; that would be a disaster
    MOV32   R0, NVIC_SYSPRI14
    MOV32   R1, NVIC_PENDSV_PRI
    STRB    R1, [R0]

    LDR     R0, =SCB_VTOR
    LDR     R0, [R0]
    LDR     R0, [R0]
    MSR     MSP, R0

    ;Todo Mpu Setup
    #if __MPU_PRESENT > 0u
        BL port_mpu_setup
    #endif

    ; k_curr_task = k_next_task
    MOV32   R0, k_curr_task
    MOV32   R1, k_next_task
    LDR     R2, [R1]
    STR     R2, [R0]

    ; sp = k_next_task->sp
    LDR     R0, [R2]
    ; PSP = sp
    MSR     PSP, R0

    ; using PSP
    MRS     R0, CONTROL
    ORR     R0, R0, #2
    MSR     CONTROL, R0

    ISB

    #if TOS_CFG_TRUSTZONE_EN > 0u
	LDMFD	SP!, {R0 - R4} ;R0 = k_curr_secure_ctx, R1=PSP, R2=PSPLIM, R3=CONTROL, R4=EXEC_RETURN 
	; task's secure context, should be 0
	; ignore EXC_RETURN
    MOV LR, R4
    LDR R4, =k_curr_secure_ctx
    STR R0, [R4]
	MSR		PSPLIM, R2
    ;MSR     PSP, R3
	#endif

    ; restore r4-11 from new process stack
    LDMFD   SP!, {R4 - R11}

    IF {FPU} != "SoftVFP" 
    ; ignore EXC_RETURN the first switch
    #if  (TOS_CFG_TRUSTZONE_EN == 0u)
    LDMFD   SP!, {R0}
    #endif
    ENDIF

    ; restore r0, r3
    LDMFD    SP!, {R0 - R3}
    ; load R12 and LR
    LDMFD    SP!, {R12, LR}
    ; load PC and discard xPSR
    LDMFD    SP!, {R1, R2}

    CPSIE    I
    BX       R1


    GLOBAL port_context_switch
port_context_switch
    LDR     R0, =NVIC_INT_CTRL
    LDR     R1, =NVIC_PENDSVSET
    STR     R1, [R0]
    BX      LR


    GLOBAL port_irq_context_switch
port_irq_context_switch
    LDR     R0, =NVIC_INT_CTRL
    LDR     R1, =NVIC_PENDSVSET
    STR     R1, [R0]
    BX      LR


    GLOBAL PendSV_Handler
PendSV_Handler
    CPSID   I ; close the interrupt
    LDR R3, =k_curr_secure_ctx              /* Read the location of k_curr_secure_ctx i.e. &( k_curr_secure_ctx ). */
    LDR R0, [R3]                            /* Read k_curr_secure_ctx - Value of k_curr_secure_ctx must be in r0 as it is used as a parameter later. */
    LDR R3, =k_curr_task                   /* Read the location of k_curr_task i.e. &( k_curr_task ). */
    LDR R1, [R3]                            /* Read k_curr_task - Value of k_curr_task must be in r1 as it is used as a parameter later. */
    ;LDR R2, [R1]                            /* R1 contains k_curr_task r2 = k_curr_task->sp*/
    MRS R2, PSP

    CBZ R0, _ns_context_save                 /* No secure context to save. */
/*@todo 需要根据 secure_context 判断是否开启了 TrustZone 模式 */

_s_context_save
    push {R0-R2, LR}
    bl tos_secure_ctx_save        /* Params are in r0 and r1. r0 = k_curr_secure_ctx and r1 = k_curr_task. */
    pop {R0-R2, LR}

_ns_context_save
    MOV R3, LR
	; test bit[6] of EXC_RETURN, determine whether non-secure stack is used
	LSLS	R3, R3, #25 ;EXC_RETURN = 0xFFFFFFFBC
	; non-secure stack is used, save NS context
	BMI	    _special_args_save	

_general_args_save
    ; R0-R3, R12, LR, PC, xPSR is saved automatically here
    IF {FPU} != "SoftVFP"
    ; is it extended frame?
    TST     LR, #0x10
    IT      EQ
    VSTMDBEQ  R2!, {S16 - S31}
    ; S0 - S16, FPSCR saved automatically here

    #if (TOS_CFG_TRUSTZONE_EN == 0u)
    ; save EXC_RETURN
    STMFD   R2!, {LR}
    #endif
    ENDIF

    ; save remaining regs r4-11 on process stack
    STMFD   R2!, {R4 - R11}


_special_args_save ;scure_stack is used, save special args:only save PSPLIM, PSP, LR
    ;LDR     R3, =k_curr_task
    MRS R3, PSP                          /* r3 = PSP. */
    MRS R4, PSPLIM                      /* r4 = PSPLIM. */
    MRS R5, CONTROL                     /* r5 = CONTROL. */
    STMFD R2!, {R0, R3-R5, LR}          /* store k_curr_secure_ctx(r0) original PSP (R3,after hardware has saved context), PSPLIM(R4), CONTROL(R5) and LR. */

_task_save_done
    ;k_curr_task->sp = new task psp 
    STR R2, [R1]                        /* Save the location from where the context should be restored as the first member of TCB. */

_mpu_program


_task_exchange
    ;k_curr_task = k_next_task
    LDR R3, =k_curr_task ;R3=&k_curr_task
    LDR R4, =k_next_task ;R4 = &k_next_task
    LDR R2, [R4] ;R2 = k_next_task
    STR R2, [R3]

_context_restore
    LDR R3, =k_next_task ;R3 = &k_next_task
    LDR R1, [R3]         ;R1 = k_next_task
    LDR R2, [R1]         ;R2 = k_next_task->sp

_special_regs_restore
    LDMFD R2!, {R0, R3-R5, LR}          /* r0 = k_curr_secure_ctx, r3 = original PSP, r4 = PSPLIM, r5 = CONTROL, LR restored. */
    ;MSR PSP, R3
    MSR PSPLIM, R4
    ;MSR CONTROL, R5
    LDR R4, =k_curr_secure_ctx /* Read the location of k_curr_secure_ctx i.e. &( k_curr_secure_ctx ). */
    STR R0, [R4]                        /* Restore k_curr_secure_ctx. */
    CBZ R0, _ns_context_restore         /* No secure context to restore. */

_s_context_restore
    PUSH {R1-R3, LR}
    BL  tos_secure_ctx_load            /* Params are in r0 and r1. r0 = k_curr_secure_ctx and r1 = k_curr_task. */
    POP {R1-R3, LR}


_ns_context_restore
    MOV R0, LR                         /* R0 = LR (EXC_RETURN). */
    LSLS  R0, R0, #25                    /* R0 = R0 << 25. Bit[6] of EXC_RETURN is 1 if secure stack was used, 0 if non-secure stack was used to store stack frame. */
    BMI _context_restore_done            /* R0 < 0 ==> Bit[6] in EXC_RETURN is 1 ==> secure stack was used to store the stack frame. */

    ; R2 = k_next_task->sp
    ; restore R4 - R11
    LDMFD   R2!, {R4 - R11}

    IF {FPU} != "SoftVFP"
    #if (TOS_CFG_TRUSTZONE_EN == 0u)
    ; restore EXC_RETURN
    LDMFD   R2!, {LR}
    #endif
    ; is it extended frame?
    TST     LR, #0x10
    IT      EQ
    VLDMIAEQ    R2!, {S16 - S31}
    ENDIF


_context_restore_done
    STR R2, [R1]                        /* Save the location where the context should be saved next as the first member . */
    ; Load PSP with new process SP
    MSR     PSP, R2
    CPSIE   I
    ; R0-R3, R12, LR, PC, xPSR restored automatically here
    ; S0 - S16, FPSCR restored automatically here if FPCA = 1
    BX LR 


#if TOS_CFG_TRUSTZONE_EN > 0u

; Function: port_secure_ctx_alloc
port_secure_ctx_alloc PROC
    SVC     #PORT_SVC_SECURE_CTX_ALLOC
    BX      LR
    ENDP

; Function: port_secure_ctx_free
port_secure_ctx_free PROC
    ; R2 contains the task's top of stack
    LDR     R2, [R0]
    ; R1 contains the task's k_secure_ctx
    LDR     R1, [R2]
    CMP     R1, #K_SECURE_CTX_INVALID_ID
    IT      NE
    SVCNE   #PORT_SVC_SECURE_CTX_FREE
    BX      LR
    ENDP

; Function: port_secure_ctx_free_curr
port_secure_ctx_free_curr PROC
    MOVW    R2, #:lower16:k_curr_secure_ctx
    MOVT    R2, #:upper16:k_curr_secure_ctx
    LDR     R1, [R2]
    CMP     R1, #K_SECURE_CTX_INVALID_ID
    IT      NE
    SVCNE   #PORT_SVC_SECURE_CTX_FREE
    BX      LR
    ENDP

; Function: port_secure_ctx_init
port_secure_ctx_init PROC
    SVC     #PORT_SVC_SECURE_CTX_INIT
    BX      LR
    ENDP


#endif

    ALIGN
    END

